import lejos.nxt.*;
import lejos.robotics.navigation.CompassPilot;
import lejos.robotics.navigation.Pilot;
import lejos.robotics.navigation.TachoPilot;

public class Pathfinder 
{
	private static final float DIAMETER = 5.6F; // cm
	private static final float TRACKWIDTH = 17.8F; // cm
	private static final Motor LEFTMOTOR = Motor.B;
	private static final Motor RIGHTMOTOR = Motor.C;
	private static final SensorPort COMPASSPORT = SensorPort.S1;
	private static final boolean REVERSE = true;
	private static final boolean CALIBRATE = true;
	
    public static void main(String [] args) throws Exception
    {    	
    	AvoidFront af;
    	WaypointDrive wd;
    	
    	Pilot pilot = new TachoPilot(DIAMETER, TRACKWIDTH, LEFTMOTOR, RIGHTMOTOR, REVERSE);
    	NavigationCar.initialize(pilot);
    	
        LCD.drawString("Pathfinder",0,0);
        LCD.refresh();
    	
        wd = new WaypointDrive ("Waypoint ",2,null, new int[] {200,0,
															   100,100,
															   100,-50,
															   0,0});
    	af = new AvoidFront ("Avoid",1,wd);
    	
    	wd.start();
       	af.start();
    	
    	while (! Button.ESCAPE.isPressed())
    	{
    		af.reportState();
    		//wd.reportState();
    	}
    	
    	LCD.clear();
        LCD.drawString("Program stopped",0,0);
        LCD.refresh();

    }    
}
